//*****************************************************************************
// File Name	: MiniOre32LF1.c
//
// Title		: MiniOre32 Line Follower Mark 1
// Revision		: See version.h for versioning info.
// Notes		: The program is makes extensive use of avrlib
// Target MCU	: Atmel ATmega32 series
// Editor Tabs	: 4
//
// Revision History:
// When			Who			Description of change
// -----------	-----------	-----------------------
// 2009.02.17 	andrzej	    Created the program
//*****************************************************************************

//----- Include Files ---------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

#include <avr/io.h>			// include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h>	// include interrupt support
#include <avr/eeprom.h>
#include <util/delay.h>

#include "global.h"		// include our global settings
#include "uart.h"		// include uart function library
#include "rprintf.h"	// include printf function library
#include "a2d.h"		// include A/D converter function library
#include "timer.h"		// include timer function library (timing, PWM, etc)
#include "vt100.h"		// include vt100 terminal support
#include "cmdline.h"	// include cmdline function library
#include "extint.h"		// include external interrupt library
#include "buffer.h"
#include "MO2012a.h"

#ifndef CRITICAL_SECTION_START
#define CRITICAL_SECTION_START	unsigned char _sreg = SREG; cli()
#define CRITICAL_SECTION_END	SREG = _sreg
#endif

#define FULLVERSION_STRING "2012.06.01 REL 1.0.0"

// Global Variables
int16_t ir_detectors[NUM_OF_SENSORS];
int8_t  center_of_mass;
int16_t corr_calc[NUM_OF_CORRELATORS];
int16_t approx_sqrt_energy;
uint8_t corr_fire[NUM_OF_CORRELATORS];

MoParameters_struct MoParam;
ControlParam_struct ControlParam;

// EEPROM and RAM
EE_RAM_CenterOfMassTaps_t __attribute__((section (".eeprom"))) EEROM_CentOfMass =
{
		-20, -12, -4, 4, 12, 20, // Center of mass taps
		32, // Calculation offset
		0, // Offset for final center of mass calculation
};

EE_RAM_CenterOfMassTaps_t CentOfMass;

EE_RAM_CorrThreshCoeff_t __attribute__((section (".eeprom"))) EEROM_CorrSettings =
{
		32, // Calculation offset
		305, // Thresholds
		285,
		311,
		317,
		0,
		8, -8, -8,  8,  8, -8, // Numerators for start/finish, 1 of 3
		8,  0, -9,  0,  8,  0, // Numerators for start/finish, 2 of 3
		6,  6, -6, -6,  6,  6, // Numerators for start/finish, 3 of 3
		4,  4,  4,  4,  4,  4, // Numerators for level crossing detection
		1,  1,  1,  1,  1,  1, // unused
};

EE_RAM_CorrThreshCoeff_t CorrSettings;

//-------------------------------------------------------------------------
// Default parameters saved in EEROM
MoParameters_struct __attribute__((section (".eeprom"))) EEROM_MoParam =
{
		1,      // uint8_t  mode_of_operation;
		0,      // uint8_t  debug;
		255,    // uint8_t  max_pwm;
	    10,0,0, // int16_t  k[3];
	    1,      // uint8_t  loop_delay;
};

uint8_t state_transition_threshold[2];
static int16_t error_vector[ERROR_BUFFER_SIZE];
int16Buffer_t ErrorBuffer;

// Sensors_t Sensors;

//----- House keeping and hardware initialization -----------------------------
int main(void)
{
	uartInit(); //Initialize UART
	uartSetBaudRate(115200);//Default Baudrate
	rprintfInit(uartSendByte);
	cmdlineInit();
	cmdlineSetOutputFunc(uartSendByte);
	vt100Init();
	vt100ClearScreen();

	a2dInit();
	// initialize the timer system
	timerInit();
	timer0SetPrescaler(TIMER_CLK_DIV64);
	timer1SetPrescaler(TIMER_CLK_DIV1);

	// set none-inverting mode
	TCCR0A |= (1 << COM0A1);
	TCCR0A |= (1 << COM0B1);

	// set fast PWM Mode
	TCCR0A |= (1 << WGM01) | (1 << WGM00);
	// set prescaler to 8 and starts PWM
	TCCR0B |= (1 << CS01);

	// Configure Pin Direction
	DDRB = _BV(MOTB_DIR1);
	DDRD = _BV(MOTB_DIR2) | _BV(MOTA_PWM) | _BV(MOTB_PWM)
			| _BV(MOTA_DIR1) | _BV(MOTA_DIR2);

	// **********************************************************
	// PORT C
	// configure a2d port (PORTC) as input
	// so we can receive analog signals
	DDRC = 0x00;

	// make sure pull-up resistors are turned off
	PORTC = 0x00;

	// set the a2d prescaler (clock division ratio)
	// - a lower prescale setting will make the a2d converter go faster
	// - a higher setting will make it go slower but the measurements
	//   will be more accurate
	// - other allowed prescale values can be found in a2d.h
	a2dSetPrescaler(ADC_PRESCALE_DIV8);

	// set the a2d reference
	// - the reference is the voltage against which a2d measurements are made
	// - other allowed reference values can be found in a2d.h
	a2dSetReference(ADC_REFERENCE_AVCC);

	InitializeVariables();

	// start command line
	RumCmdLine();

	return 0;
}

void InitializeVariables(void) ///Initialize motor drive PID variables
{
	// Copy EEROM contents to RAM
	eeprom_read_block(&MoParam, &EEROM_MoParam, sizeof(EEROM_MoParam));
	eeprom_read_block(&CentOfMass, &EEROM_CentOfMass, sizeof(EEROM_CentOfMass));
	eeprom_read_block(&CorrSettings, &EEROM_CorrSettings, sizeof(EEROM_CorrSettings));

	//Initialize ErrorBuffer
	ErrorBufferInit(&ErrorBuffer, error_vector, ERROR_BUFFER_SIZE);

	ControlParam.error          = 0;
	ControlParam.error_last     = 0;
	ControlParam.error_integral = 0;
	ControlParam.pwm[0]         = 0;
	ControlParam.pwm[1]         = 0;
	ControlParam.dir[0]         = 0;
	ControlParam.dir[1]         = 0;

	state_transition_threshold[0] = 10;
	state_transition_threshold[1] = 5;
}

void RumCmdLine(void)
{
	u08 c;

	// print welcome message
	vt100ClearScreen();
	vt100SetCursorPos(1, 0);
	idn();

	// initialize cmdline system
	cmdlineInit();

	// direct cmdline output to uart (serial port)
	cmdlineSetOutputFunc(uartSendByte);

	// add commands to the command database
	cmdlineAddCommand("help", ShellHelp);
	cmdlineAddCommand("idn",  idn);
	cmdlineAddCommand("pwm",  ShellSetMotorPwm);
	cmdlineAddCommand("bufprint", ShellPrintBuffer);
	cmdlineAddCommand("bufadd",   ShellAddToBuffer);
	cmdlineAddCommand("test", test);
	cmdlineAddCommand("get",  ShellGetParams);
	cmdlineAddCommand("set",  ShellSetParam);
	cmdlineAddCommand("run",  Run);
	cmdlineAddCommand("adc",  ShellAdc);
	cmdlineAddCommand("adcd", DisplaySensors);
	cmdlineAddCommand("mon",  ShellMonitor);
	cmdlineAddCommand("rc",   ManualDrive);
	cmdlineAddCommand("corr", ShellGetCorrelators);

	// send a CR to cmdline input to stimulate a prompt
	cmdlineInputFunc('\r');

	// main loop handle command line input
	while (1)
	{
		// pass characters received on the uart (serial port)
		// into the cmdline processor
		while (uartReceiveByte(&c))
			cmdlineInputFunc(c);
		cmdlineMainLoop();
	}
	rprintfCRLF();
	rprintfProgStrM("Exited program.  Press reset to restart.\r\n");
}
//////////////////////////////////////////////////////////////////////////////////
// Shell Commands
void ShellHelp(void)
{
	rprintfProgStrM("help - print help page\n\n");
	rprintfProgStrM("adc  - returns values of the ADC sensors in html/json format\n");
	rprintfProgStrM("adcd - debug adc related\n");
	rprintfProgStrM("cor  - returns all correlator parameters\n");
	rprintfProgStrM("get  - returns all MoParameters\n");
	rprintfProgStrM("set  - set specific MoParameters\n");
	rprintfProgStrM("mon  - run real time debugging mode\n");
	rprintfProgStrM("idn  - firmware version\n");
	rprintfProgStrM("pwm  - set motor, direction and pwm\n");
	rprintfProgStrM("test - generic testing function\n");
	rprintfProgStrM("run  - run the main program loop\n");
	rprintfProgStrM("rc   - remote control\n");
}
void ShellSetMotorPwm(void)
{
	//Manually sets the PWM value
	uint8_t motor = (uint8_t) cmdlineGetArgInt(1);
	uint8_t dir   = (uint8_t) cmdlineGetArgInt(2);
	uint16_t pwm  = (uint8_t) cmdlineGetArgInt(3);

	//TODO - add input parameter validation
	rprintf("ShellSetMotorPwm(motor =%d, dir=%d, pwm=%d\n", motor, dir, pwm);
	SetMotorPwm(motor, dir, pwm);
}
void ShellPrintBuffer(void)
{
	ErrorBufferPrint(&ErrorBuffer);
}
void ShellAddToBuffer(void)
{
	uint8_t val = (int16_t) cmdlineGetArgInt(1);
	ErrorBufferAddToEnd(&ErrorBuffer, val);
}
void ShellGetCorrelators(void)
{
	clc();
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM("20: calc_offset    :");
	rprintfNum(10, 8, TRUE, ' ', CorrSettings.calc_offset);
	rprintfCRLF();

	rprintfProgStrM("21: corr_thresh    :");
	PrintArray16(NUM_OF_CORRELATORS, CorrSettings.corr_thresh, TRUE);
	rprintfCRLF();

	for(uint8_t i = 0; i<NUM_OF_CORRELATORS; i++)
	{
		rprintf("%d: corr_coeff_num :",22+i);
		PrintArray16(NUM_OF_SENSORS, CorrSettings.corr_coeff_num[i], TRUE);
		rprintfCRLF();
	}
//	rprintfProgStrM("11: corr_coeff_num :");
//	rprintfNum(10, 8, TRUE, ' ', CorrSettings.corr_coeff_num);
//	rprintfCRLF();
}
void ShellGetParams(void)
{
	clc();
	vt100SetAttr(VT100_BOLD);
	vt100SetAttr(VT100_GREEN);
	rprintfProgStrM(" 1: mode_of_operation :");
	rprintfNum(10, 6, FALSE, ' ', MoParam.mode_of_operation);
	if (MoParam.mode_of_operation) {
		rprintfProgStrM(" MODE_LINE_FOLLOWER");
		rprintfCRLF();
	} else {
		rprintfProgStrM(" MODE_LINE_RACER");
		rprintfCRLF();
	}
	vt100SetAttr(0);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM(" 2: debug             :");
	rprintfNum(10, 6, FALSE, ' ', MoParam.debug);
	rprintfCRLF();

	rprintfProgStrM(" 3: max_pwm           :");
	rprintfNum(10, 6, FALSE, ' ', MoParam.max_pwm);
	rprintfCRLF();

	rprintfProgStrM(" 4: gain              :");
	rprintfNum(10, 6, FALSE, ' ', MoParam.k[0]);
	rprintfNum(10, 6, FALSE, ' ', MoParam.k[1]);
	rprintfNum(10, 6, FALSE, ' ', MoParam.k[2]);
	rprintfCRLF();

	rprintfProgStrM(" 5: loop_delay        :");
	rprintfNum(10, 6, FALSE, ' ', MoParam.loop_delay);
	rprintfCRLF();

	//Center of mass parameters
	rprintfProgStrM(" 6: CofM taps         :");
	PrintArray16(NUM_OF_SENSORS, CentOfMass.taps, TRUE);
	rprintfCRLF();

	rprintfProgStrM(" 7: CofM calc_offset  :");
	rprintfNum(10, 6, FALSE, ' ', CentOfMass.calc_offset);
	rprintfCRLF();

	rprintfProgStrM(" 8: CofM offset       :");
	rprintfNum(10, 6, TRUE, ' ', CentOfMass.offset);
	rprintfCRLF();

	rprintfProgStrM(" 9: state_trans_thr   :");
	rprintfNum(10, 6, FALSE, ' ', state_transition_threshold[0]);
	rprintfNum(10, 6, FALSE, ' ', state_transition_threshold[1]);
	rprintfCRLF();

}
void ShellSetParam(void)
{
	uint8_t selection = cmdlineGetArgInt(1);
		//uint8_t i;
		cli();
		switch (selection)
		{
		case 1:
			MoParam.mode_of_operation = (uint8_t) cmdlineGetArgInt(2);
			eeprom_write_byte(&EEROM_MoParam.mode_of_operation, MoParam.mode_of_operation);
			break;
		case 2:
			MoParam.debug = (uint8_t) cmdlineGetArgInt(2);
			eeprom_write_byte(&EEROM_MoParam.debug, MoParam.debug);
			break;
		case 3:
			MoParam.max_pwm = (uint8_t) cmdlineGetArgInt(2);
			eeprom_write_byte(&EEROM_MoParam.max_pwm, MoParam.max_pwm);
			break;
		case 4:
			MoParam.k[0] = (MoParam.k[0] = (int16_t) cmdlineGetArgInt(2));
			eeprom_write_word(&EEROM_MoParam.k[0], MoParam.k[0]);
			MoParam.k[1] = (MoParam.k[1] = (int16_t) cmdlineGetArgInt(3));
			eeprom_write_word(&EEROM_MoParam.k[1], MoParam.k[1]);
			MoParam.k[2] = (MoParam.k[2] = (int16_t) cmdlineGetArgInt(4));
			eeprom_write_word(&EEROM_MoParam.k[2], MoParam.k[2]);
			break;
		case 5:
			MoParam.loop_delay = (uint16_t) cmdlineGetArgInt(2);
			eeprom_write_word(&EEROM_MoParam.loop_delay, MoParam.loop_delay);
			break;

		case 6:
			for(uint8_t i=0;i<NUM_OF_SENSORS;i++)
				CentOfMass.taps[i] = (int16_t) cmdlineGetArgInt(i+2);
			eeprom_write_block(&CentOfMass, &EEROM_CentOfMass, sizeof(EEROM_CentOfMass));
			//eeprom_write_word(&EEROM_MoParam.loop_delay, MoParam.loop_delay);
			break;
		case 7:
			CentOfMass.calc_offset = (int16_t) cmdlineGetArgInt(2);
			eeprom_write_word(&CentOfMass.calc_offset, EEROM_CentOfMass.calc_offset);
			break;
		case 8:
			CentOfMass.offset = (int16_t) cmdlineGetArgInt(2);
			eeprom_write_word(&CentOfMass.offset, EEROM_CentOfMass.offset);
			break;
		case 9:
			state_transition_threshold[cmdlineGetArgInt(2)] = cmdlineGetArgInt(3);
			break;
		case 21:
			(int16_t) cmdlineGetArgInt(2);
			CorrSettings.corr_thresh[cmdlineGetArgInt(2)] = cmdlineGetArgInt(3);
			eeprom_write_block(&CorrSettings, &EEROM_CorrSettings, sizeof(EEROM_CorrSettings));
			break;

		default:
			vt100SetAttr(VT100_RED);
			rprintfProgStrM("Invalid Option.");
			vt100SetAttr(0);
			rprintfCRLF();
			break;
		}
		sei();

		if (selection < 20)
			ShellGetParams();
		else if(selection >= 20)
			ShellGetCorrelators();


}
void ShellAdc(void)
{
	ReadSensors();
	rprintfProgStrM("<json>[");
	for (uint8_t j = 0; j < NUM_OF_SENSORS; j++)
		{
			rprintf("%d",ir_detectors[j]);
			if(j < NUM_OF_SENSORS -1)
				rprintfProgStrM(",");
		}
	rprintfProgStrM("]</json>\n");

}
void ShellMonitor(void)
{
	char c;
	uint8_t done = 0;
	uint8_t motors_on = 0;
	timer0ClearOverflowCount();
	clc();
	vt100SetCursorMode(0);
	ControlParam.the_state = STATE_START_SEARCH;
	ControlParam.corr_count = 0;

	while (!done)
	{
		uartReceiveByte(&c);
		PrintMonitorPage();
		if(motors_on)
		{
			SetMotorPwm(LEFT,  1, ControlParam.pwm[0]);
			SetMotorPwm(RIGHT, 1, ControlParam.pwm[1]);
		}

		rprintfCRLF();
		rprintfProgStrM("MOTOR        : ");
		rprintfNum(10, 8, FALSE, ' ', motors_on);
		rprintfCRLF();
		rprintfProgStrM("c            : ");
		rprintfNum(10, 8, FALSE, ' ', c);
		rprintfCRLF();

		switch (c)
		{
		case 'q':
			done = 1;
			break;
		case 'l':
			toggle(PORTD,PB5);
			c = 0;
			break;
		case 'm':
			if(motors_on == 0)
				motors_on = 1;
			else
			{
				motors_on = 0;
				SetMotorPwm(LEFT,  0, 0);
				SetMotorPwm(RIGHT, 0, 0);
			}
			c = 0;
			break;
		default:
			c = 0;
		}
		timerPause(100);
	}
	vt100SetAttr(0);
	vt100SetCursorMode(1);
	rprintfCRLF();
	SetMotorPwm(LEFT,  0, 0);
	SetMotorPwm(RIGHT, 0, 0);
}
//////////////////////////////////////////////////////////////////////////////////
// Utility functions
void PrintMonitorPage(void)
{
	//Function for real time display of sensors and other monitors
	uint16_t timer_start, timer_stop;
	uint16_t adc_conversion_time;
	uint8_t top_speed = MoParam.max_pwm;

	timer_start = TCNT1;
	ReadSensors();
	timer_stop  = TCNT1;
	adc_conversion_time = timer_stop - timer_start;
	GetCurrentState();
	switch(ControlParam.the_state)
	{
	case STATE_START_SEARCH:
		//MoParam.max_pwm = 100;
		break;
	case STATE_RUN:
		//MoParam.max_pwm = top_speed;
		break;
	case STATE_FINISH:
		break;
	default:
		break;
	}
	GetNewPwmValues();

	vt100SetCursorPos(2, 1);
	vt100SetAttr(VT100_BOLD);
	rprintfProgStrM("TIMER0 OVF   : ");
	rprintfNum(10, 5, FALSE, ' ', timer0GetOverflowCount());
	rprintfCRLF();

	rprintfProgStrM("RAW ADC      : ");
	PrintArray16(NUM_OF_SENSORS, ir_detectors, FALSE);
	rprintfCRLF();

	rprintfProgStrM("ADC DURATION : ");
	rprintfNum(10, 5, FALSE, ' ', adc_conversion_time);
	rprintfCRLF();

	rprintfProgStrM("C OF MASS    : ");
	rprintfNum(10, 5, TRUE, ' ', center_of_mass);
	rprintfCRLF();

	rprintfProgStrM("Sqrt Energy  : ");
	rprintfNum(10, 5, FALSE, ' ', approx_sqrt_energy);
	rprintfCRLF();

	rprintfProgStrM("ERROR SIGNAL : ");
	rprintfNum(10, 5, TRUE, ' ', ControlParam.error);
	rprintfCRLF();

	rprintfProgStrM("PWM          : ");
	rprintfNum(10, 5, FALSE, ' ', ControlParam.pwm[0]);
	rprintfNum(10, 5, FALSE, ' ', ControlParam.pwm[1]);
	rprintfCRLF();

	rprintfCRLF();
	rprintfProgStrM("Corr         :");
	PrintArray16(NUM_OF_CORRELATORS, corr_calc, TRUE);
	rprintfCRLF();

	rprintfProgStrM("Corr thresh  :");
	PrintArray16(NUM_OF_CORRELATORS, CorrSettings.corr_thresh, TRUE);
	rprintfCRLF();

	rprintfProgStrM("present_state:");
	rprintfNum(10, 5, FALSE, ' ', ControlParam.present_state);
	rprintfCRLF();

	rprintfProgStrM("previous_state:");
	rprintfNum(10, 5, FALSE, ' ', ControlParam.previous_state);
	rprintfCRLF();

	rprintfProgStrM("Corr Fire    :");
	PrintArray(NUM_OF_CORRELATORS, corr_fire, FALSE);



	// rprintfProgStrM("NORM ADC  : ");
	// PrintArray(NUM_OF_SENSORS, Sensors.norm);
}
void PrintArray(uint8_t size, uint8_t *array, uint8_t show_sign)
{
	//Utility function for printing contents of an array
	uint8_t i;
	for (i = 0; i < size; i++)
	{
		rprintfNum(10, 5, show_sign, ' ', array[i]);
		rprintfProgStrM("  ");
	}
	rprintfCRLF();
}

void PrintArray16(uint8_t size, int16_t *array, uint8_t show_sign)
{
	uint8_t i;
	for (i = 0; i < size; i++)
	{
		rprintfNum(10, 5, show_sign, ' ', array[i]);
		rprintfProgStrM("  ");
	}
	//rprintfCRLF();
}

void BlinkStatusLed(uint8_t num_of_times, uint16_t delay)
{
	uint8_t i;
	for (i = 0; i < num_of_times; i++)
	{
		toggle(PORTC,STATUS_LED);
		timerPause(delay);
		toggle(PORTC,STATUS_LED);
		timerPause(delay);
	}
}
void idn(void)  ///Prints version information
{
	//Responds to a standard instrumentation *IDN? query
	rprintfProgStrM("MOvember Line Follower Version: ");
	rprintfProgStrM(FULLVERSION_STRING);
	rprintfCRLF();
}
void test(void) //Generic test function
{
	rprintfProgStrM("Test function\r\n");
	;
}

void DisplaySensors(void) // Print sensor reading
{
	uint8_t j;
	uint16_t timer_start, timer_stop, adc_conversion_time;


	// Have the ADCs read
	timer_start = TCNT1;
	// Get the ADC reading
	ReadSensors();
	timer_stop = TCNT1;
	adc_conversion_time = timer_stop - timer_start;

	rprintfProgStrM("ReadSensors ");
	rprintfNum(10, 6, TRUE, ' ', timer_start);
	rprintfNum(10, 6, TRUE, ' ', timer_stop);
	rprintfCRLF();

	// Print the output string
	rprintfProgStrM("ADC Values: ");
	// Print each ADC reading
	for (j = 0; j < NUM_OF_SENSORS; j++) {
		rprintfNum(10, 6, TRUE, ' ', ir_detectors[j]);
	}
	rprintfCRLF();

	/*
	for (j = 0; j < NUM_OF_SENSORS; j++) {
		rprintfNum(10, 6, TRUE, ' ', CentOfMass.taps[j]);
	}
	rprintfCRLF();
	*/

	rprintfProgStrM("Center of Mass: ");
	rprintfNum(10, 6, TRUE, ' ', center_of_mass);
	rprintfCRLF();
}
void ReadSensors(void)
{
	uint8_t j, i;
	int16_t working_adc, corr_adc, sum_adc, sum_adc_com;

	// For each sensor, get the signed, 16 bit representation
	// Also, calculations
	sum_adc = 0;
	sum_adc_com = 0;
	approx_sqrt_energy = 0;
	for (j = 0; j < NUM_OF_SENSORS; j++) {
		// Get the IR detectors ADC values
		ir_detectors[j] = (int16_t) (a2dConvert10bit(j));
		// The working ir adc value
		working_adc = ir_detectors[j] - CentOfMass.calc_offset;
		corr_adc = ir_detectors[j] - CorrSettings.calc_offset;
		// Sum the ir adc values
		sum_adc = sum_adc + working_adc;
		// Sum the ir adc values, scaled by the center of mass coefficients
		sum_adc_com = sum_adc_com + working_adc * CentOfMass.taps[j];
		// Calculate the approximate square root
		approx_sqrt_energy = approx_sqrt_energy + (corr_adc / 16) * (corr_adc / 16);

		// Finalize the approximate square root
		if (j == (NUM_OF_SENSORS - 1))
			approx_sqrt_energy = approx_sqrt_energy / 449 + 24;

		// For each correlator
		for (i = 0; i < NUM_OF_CORRELATORS; i++) {
			// Set the correlator calculation back to 0
			if (j == 0) {
				corr_calc[i] = 0;
			}

			// Update the correlator calculation
			corr_calc[i] = corr_calc[i] + corr_adc * CorrSettings.corr_coeff_num[i][j];

			// Complete the correlation calculation
			if (j == (NUM_OF_SENSORS - 1)) {
				corr_calc[i] = corr_calc[i] / approx_sqrt_energy;

				if (corr_calc[i] > CorrSettings.corr_thresh[i])
					corr_fire[i] = 1;
				else
					corr_fire[i] = 0;
			}

		}
	}

	// Scale the ADC sum
	sum_adc = sum_adc >> 2;
	// Center of mass
	if (sum_adc != 0) {
		center_of_mass = (int8_t) ((sum_adc_com / sum_adc - CentOfMass.offset));
	}
	else
		center_of_mass = 0;
}
// Buffer functions and operations
void ErrorBufferInit(int16Buffer_t *buffer, uint8_t *start, uint16_t size)
{
	// begin critical section
	CRITICAL_SECTION_START;
	// set start pointer of the buffer
	buffer->dataptr = start;
	buffer->size = size;
	// initialize index and length
	buffer->datalength = 0;
	buffer->dataindex = 0;
	// end critical section
	CRITICAL_SECTION_END;
}
unsigned char ErrorBufferAddToEnd(int16Buffer_t* buffer, int16_t data)
{
	// begin critical section
	CRITICAL_SECTION_START;
	// make sure the buffer has room
	// save data byte at end of buffer
	buffer->dataptr[buffer->dataindex] = data;
	buffer->dataindex++;
	if (buffer->dataindex >= buffer->size)
		buffer->dataindex = 0;
	// end critical section
	CRITICAL_SECTION_END;
	// return success
	return 0;
}
int16_t ErrorBufferGetValues(int16Buffer_t* buffer, uint16_t index)
{
	if (index >= buffer->size - 1)
		index = buffer->size - 1;

	if (index < buffer->dataindex)
		index = buffer->dataindex - index - 1;
	else
		index = buffer->size - index + buffer->dataindex - 1;
	CRITICAL_SECTION_START;
	int16_t data = buffer->dataptr[index];// return character at index in buffer
	CRITICAL_SECTION_END; // end critical section
	//rprintf("Index: %d = %d.\r\n",index,data);
	return data;
}
void ErrorBufferPrint(int16Buffer_t *buffer)
{
	uint8_t i;
	rprintfProgStrM("<json>[");
	for (i = 0; i < buffer->size; i++)
	{
		rprintf("%d",buffer->dataptr[i]);
		if(i < buffer->size -1)
			rprintfProgStrM(",");
	}
	rprintfProgStrM("]</json>");
	rprintfCRLF();
}
//////////////////////////////////////////////////////////////////////////////////
// MOTOR CONTROL
void SetMotorPwm(uint8_t motor, uint8_t dir, uint16_t pwm) ///Set specified motor direction and speed
{
	// Truth table for driving TB6552FN/FNG/FL/FLG
	// Taken from data sheet page 3
	// INPUTS           OUTPUTS MODE
	//IN1 IN2 STBY PWM  O1 O2
	//L   H   H    H    L  H    CW
	//H   L   H    H    L  H    CCW
	//L   L   H    L    L  H    STOP
	//H   H   H    H/L  L  L    Short brake
	//L   H   H    L    L  L    Short brake
	//H   L   H    L    L  H    Short brake

	if (pwm >= MoParam.max_pwm)
		pwm = MoParam.max_pwm;

	switch (motor)
		{
		case LEFT:
			//rprintf("Motor 1: ");
			if (dir == FORWARD)
			{
				//rprintf("FORWARD \n");
				cbi(PORTD,MOTA_DIR1);
				sbi(PORTD,MOTA_DIR2);
			}
			else if (dir == REVERSE)
			{
				//rprintf("REVERSE \n");
				sbi(PORTD,MOTA_DIR1);
				cbi(PORTD,MOTA_DIR2);
			}
			else
			{
				cbi(PORTD,MOTA_DIR1);
				cbi(PORTD,MOTA_DIR2);
				pwm = 0;
			}
			OCR0A = pwm;
			break;
		case RIGHT:
			//rprintf("Motor 2: ");
			if (dir == FORWARD)
			{
				//rprintf("FORWARD \n");
				cbi(PORTB,MOTB_DIR1);
				sbi(PORTD,MOTB_DIR2);
			}
			else if (dir == REVERSE)
			{
				//rprintf("REVERSE \n");
				sbi(PORTB,MOTB_DIR1);
				cbi(PORTD,MOTB_DIR2);
			}
			else
			{
				cbi(PORTB,MOTB_DIR1);
				cbi(PORTD,MOTB_DIR2);
				pwm = 0;
			}
			OCR0B = pwm;
			break;
		default:
			OCR0A = 0;
			OCR0B = 0;
		}
}
void ManualDrive(void)
{
	// UNDER CONSTRUCTION
	// function will be used for remote control of MO using buttons w,a,s,d,x
	char c;
	uint8_t pwm[2], dir[2], top_speed = MoParam.max_pwm;
	int16_t done = 0;
	rprintfProgStrM("ManualDrive\n");
	for(uint8_t i;i<2;i++)
	{
		pwm[i] = 0;
		dir[i] = 1;
	}

	while (!done)
	{
		uartReceiveByte(&c); //Monitor characters in the Rx buffer
		toggle(PORTB,PB5); //Status pin flip, can also be used to measure loop time with a scope

		switch (c)
		{
		case 'w':
			pwm[0] += 25;
			pwm[1] += 25;
			dir[0] = dir[1]=1;
			rprintf("%d %d\n",pwm[0],pwm[1]);
			c = ' ';
			break;
		case 'x':
			pwm[0] += 25;
			pwm[1] += 25;
			dir[0] = dir[1]=0;
			rprintf("%d %d\n",pwm[0],pwm[1]);
			c = ' ';
			break;
		case 'd':
			pwm[0] -= 25;
			pwm[1] += 25;
			rprintf("%d %d\n",pwm[0],pwm[1]);
			c = ' ';
			break;
		case 'a':
			pwm[1] -= 25;
			pwm[0] += 25;
			rprintf("%d %d\n",pwm[0],pwm[1]);
			c = ' ';
			break;
		case 's':
			pwm[0]=pwm[1]=0;
			c = ' ';
			break;
		case 'q':
			done = 1;
			break;
		default:
			break;
		}
		SetMotorPwm(1, dir[0], pwm[0]);
		SetMotorPwm(2, dir[1], pwm[1]);
	}
	SetMotorPwm(0, 0, 0);

	vt100SetAttr(0);
	vt100SetCursorMode(1);
	rprintfProgStrM("Main loop done\n");
}
void PrintDebug(void)
{
	switch(MoParam.debug)
	{
	case 1:
		rprintf("%d",timer0GetOverflowCount());
		break;
	case 2:
		rprintf("%d",ControlParam.error);
		break;
	default:
		break;
	}
}
//////////////////////////////////////////////////////////////////////////////////
// MAIN LOOP
void Run(void)
{
	char c;
	uint8_t top_speed = MoParam.max_pwm;
	uint8_t negative  = 0;
	int16_t error     = 0;

	timer0ClearOverflowCount();
	clc();
	vt100SetCursorMode(0);

	rprintfProgStrM("Main loop, press 'q' to quit\n");
	// Add <json> tags around debug data to enable easy parsing using python json module
	if (MoParam.debug)
		rprintfProgStrM("<json>[");

	ControlParam.the_state = STATE_START_SEARCH;
	ControlParam.corr_count = 0;

	while (1)
	{
		uartReceiveByte(&c); //Monitor characters in the Rx buffer

		toggle(PORTB,PB5);  //Status pin flip, can also be used to measure loop time with a scope

		ReadSensors();      //Handles ADC readings and CM calculations
		GetCurrentState();
		switch(ControlParam.the_state)
		{
		case STATE_START_SEARCH:
			MoParam.max_pwm = 100;
			break;
		case STATE_RUN:
			MoParam.max_pwm = top_speed;
			break;
		case STATE_FINISH:
			c = 'q';
			break;
		default:
			break;
		}
		GetNewPwmValues();

		SetMotorPwm(LEFT,  ControlParam.dir[0], ControlParam.pwm[0]);
		SetMotorPwm(RIGHT, ControlParam.dir[1], ControlParam.pwm[1]);

		PrintDebug();
		if(MoParam.debug) rprintfProgStrM(",");

		if (c == 'q')
		{
			MoParam.max_pwm = top_speed;
			PrintDebug();
			break;
		}
//		while ((timer1Get1verflowCount() - time1) < MoParam.loop_delay)
//		{
//		}
		//timerPause(MoParam.loop_delay);
	}

	if (MoParam.debug)
	{
		//PrintDebug(MoParam.debug, error);
		rprintfProgStrM("]</json>");
	}

	vt100SetAttr(0);
	vt100SetCursorMode(1);
	rprintfProgStrM("Main loop done\n");
	OCR0A = OCR0B = 0;
}
//////////////////////////////////////////////////////////////////////////////////
// MAIN LOOP SUBROUTINES
void GetCurrentState(void)
{
	if(corr_fire[0] || corr_fire[1] || corr_fire[2])
	{
		ControlParam.present_state = STATE_LINE_MARKER;
	}
	else if(corr_fire[0])
		ControlParam.present_state = STATE_LINE_CROSSING;
	else
		ControlParam.present_state = STATE_TRACKING;

	////////////////////
	if((ControlParam.present_state == ControlParam.previous_state) && (ControlParam.present_state == STATE_LINE_MARKER))
		ControlParam.corr_count++;
	else
		ControlParam.corr_count = 0;
	////////////////////
	if((ControlParam.corr_count >= state_transition_threshold[0]) && (ControlParam.the_state == STATE_START_SEARCH))
	{
		ControlParam.corr_count = 0;
		ControlParam.the_state = STATE_RUN;
	}
	else if((ControlParam.corr_count >= state_transition_threshold[1]) && (ControlParam.the_state == STATE_RUN))
	{
		ControlParam.corr_count = 0;
		ControlParam.the_state = STATE_FINISH;
	}
	ControlParam.previous_state = ControlParam.present_state;
}
void GetNewPwmValues(void)
{
	int16_t pwm_temp = 0;
	ControlParam.error = (((int16_t)center_of_mass) * ((int16_t)MoParam.k[0])) / 10;

	if(ControlParam.error < 0)
	{
		ControlParam.error = -ControlParam.error;
		if((ControlParam.error) > (int16_t)(MoParam.max_pwm))
			ControlParam.error = (int16_t)MoParam.max_pwm;

		ControlParam.pwm[0] = MoParam.max_pwm;
		ControlParam.pwm[1] = MoParam.max_pwm - (uint8_t)(ControlParam.error);
		ControlParam.dir[0] = 1;
		ControlParam.dir[1] = 1;
	}
	else
	{
		if((ControlParam.error) > (int16_t)(MoParam.max_pwm))
			ControlParam.error = (int16_t)MoParam.max_pwm;

		ControlParam.pwm[0] = MoParam.max_pwm - (uint8_t)(ControlParam.error);
		ControlParam.pwm[1] = MoParam.max_pwm;
		ControlParam.dir[0] = 1;
		ControlParam.dir[1] = 1;
	}
	ControlParam.error_last = ControlParam.error;
	ControlParam.error_integral += ControlParam.error;

}
